#include "app_motion.hpp"
#include <math.h>

#define GPIO_STATE_INVERT(a, x) (GPIO_PinState)(a ? !x : x)

Motor::motor_desc_t Motor::motor_desc_map[MAX_SUPPORT_MOTOR_NUM];
static uint8_t cur_motors_cnt = 0;

Motor::Motor(pwm_t pwm, gpio_t pinA, gpio_t pinB, gpio_t enc_pinA, gpio_t enc_pinB, IRQn_Type enc_irq, bool isInv)
    : pwm(pwm),
      pinA(pinA),
      pinB(pinB),
      enc_pinA(enc_pinA),
      enc_pinB(enc_pinB),
      enc_irq(enc_irq),
      isInv(isInv)
// ,pid(1.5, 1, 0.01, 0.8)
{

    HAL_TIM_Base_Start_IT(pwm.tim);
    HAL_TIM_PWM_Start(pwm.tim, pwm.channel);
    motor_desc_map[cur_motors_cnt].enc_pin = enc_pinA.pin;
    motor_desc_map[cur_motors_cnt].motor = this;
    cur_motors_cnt++;
    enc_cnt = 0;
}
Motor::~Motor()
{
    if (cur_motors_cnt <= 1)
    {
        cur_motors_cnt = 0;
        return;
    }

    for (uint8_t i = 0; i < cur_motors_cnt; i++)
    {
        if (motor_desc_map[i].enc_pin == enc_pinA.pin)
        {
            motor_desc_map[i] = motor_desc_map[cur_motors_cnt - 1];
            cur_motors_cnt--;
            break;
        }
    }
}

void Motor::set_torque(float torque)
{
    if (torque < 0)
    {
        torque = -torque;
        set_moode(MODE_BACKWARD);
    }
    else if (fabsf(torque) < 1e-5f)
    {
        set_moode(MODE_NETURAL);
    }
    else
    {
        set_moode(MODE_FORWARD);
    }

    if (torque > DETECT_MAX_ANGLE_VOLOCITY)
        torque = DETECT_MAX_ANGLE_VOLOCITY;

    uint16_t pulse_us = (uint16_t)(torque / DETECT_MAX_ANGLE_VOLOCITY * this->pwm.tim->Init.Period);
    __HAL_TIM_SET_COMPARE(this->pwm.tim, this->pwm.channel, pulse_us);
}
void Motor::set_moode(Motor_Mode mode)
{
    switch (mode)
    {
    case MODE_FORWARD:
        HAL_GPIO_WritePin(pinA.port, pinA.pin, GPIO_STATE_INVERT(isInv, GPIO_PIN_SET));
        HAL_GPIO_WritePin(pinB.port, pinB.pin, GPIO_STATE_INVERT(isInv, GPIO_PIN_RESET));
        break;
    case MODE_BACKWARD:
        HAL_GPIO_WritePin(pinA.port, pinA.pin, GPIO_STATE_INVERT(isInv, GPIO_PIN_RESET));
        HAL_GPIO_WritePin(pinB.port, pinB.pin, GPIO_STATE_INVERT(isInv, GPIO_PIN_SET));
        break;
    case MODE_NETURAL:
        HAL_GPIO_WritePin(pinA.port, pinA.pin, GPIO_PIN_RESET);
        HAL_GPIO_WritePin(pinB.port, pinB.pin, GPIO_PIN_RESET);
        break;
    case MODE_STOP:
        HAL_GPIO_WritePin(pinA.port, pinA.pin, GPIO_PIN_SET);
        HAL_GPIO_WritePin(pinB.port, pinB.pin, GPIO_PIN_SET);
        break;
    default:
        break;
    }
}
float Motor::get_angle_volocity()
{
    HAL_NVIC_DisableIRQ(this->enc_irq);
    uint32_t cur_us = get_us();

    float T = _max(this->intrupt_us - this->last_intrupt_us, cur_us - this->intrupt_us) / 1.0e6f;

    HAL_NVIC_EnableIRQ(this->enc_irq);

    float raw = (float)this->direction * PULSE_DELTA_ANGLE / T;

    /*Filter the raw volocity*/
    const float alpha = 0.3; /*越小越平滑，响应越慢（0.1~0.3都可以试试）*/
    if (fabsf(raw - last_valid_volocity) > 0.1f and
        fabsf(last_valid_volocity) > 0.03f and
        fabsf(raw) < 0.01)
    {
        return last_valid_volocity;
    }
    last_valid_volocity = alpha * raw + (1.0f - alpha) * last_valid_volocity;

    return last_valid_volocity;
}
float Motor::get_linear_volocity()
{
    return get_angle_volocity() * WHEEL_DIAMETER;
}

void Motor::handle_enc_a_interrupt()
{
    if (GPIO_STATE_INVERT(isInv, GPIO_PIN_RESET) == HAL_GPIO_ReadPin(enc_pinB.port, this->enc_pinB.pin))
    {
        this->direction = 1;
        // this->enc_cnt++;
        // selfHandler::cout << "enc_cnt++ = " << this->enc_cnt << selfHandler::endl;
    }
    else
    {
        this->direction = -1;
        // this->enc_cnt--;
        // selfHandler::cout << "enc_cnt-- = " << this->enc_cnt << selfHandler::endl;
    }

    this->last_intrupt_us = this->intrupt_us;
    this->intrupt_us = get_us();
}

void Motor::poll(float dt, float target_angle_volocity)
{
    if (target_angle_volocity == -12345.54321f)
        target_angle_volocity = this->target_angle_volocity;

    float cur_speed = get_angle_volocity();
    float output = pid->update(target_angle_volocity, cur_speed, dt) + target_angle_volocity;

    set_torque(output);
    // selfHandler::cout.set_precision(4) << cur_speed << ',' << error << ',' << output;
}

extern "C" void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    for (uint8_t i = 0; i < cur_motors_cnt; ++i)
    {
        if (Motor::motor_desc_map[i].enc_pin == GPIO_Pin)
        {
            Motor::motor_desc_map[i].motor->handle_enc_a_interrupt();
            break;
        }
    }
}
